﻿#include "RobotServerCmd.h"
#include <QDebug>
#include <QFile>
#include <QSharedMemory>
#include <string>
#include <vector>
#include "JsonUtil.h"
#include "types.h"
using namespace std;

static bool s_bMsgFlag1 = false;
static double s_lq1;
static double s_lq2;
static double s_lq3;
static double s_lq4;
static double s_lq5;
static double s_lq6;
static double s_lq7;
static double s_lx;
static double s_ly;
static double s_lz;
static double s_lrx;
static double s_lry;
static double s_lrz;
static double s_lelbow;
static double s_lFT1;
static double s_lFT2;
static double s_lFT3;
static double s_lFT4;
static double s_lFT5;
static double s_lFT6;
static quint16 s_ljState1;
static quint16 s_ljState2;
static quint16 s_ljState3;
static quint16 s_ljState4;
static quint16 s_ljState5;
static quint16 s_ljState6;
static quint16 s_ljState7;
static quint16 s_lEnable;
static quint16 s_lerrorClean;
static quint16 s_lcontrolMode;
static quint16 s_lftMode;
static quint16 s_ltriggerSignal;
static quint16 s_lperiodMode;
static double s_lperiodValue;
static double s_ltoolmass;
static double s_ltoolComX;
static double s_ltoolComY;
static double s_ltoolComZ;
static double s_lqTarget1;
static double s_lqTarget2;
static double s_lqTarget3;
static double s_lqTarget4;
static double s_lqTarget5;
static double s_lqTarget6;
static double s_lqTarget7;
static double s_lxTarget;
static double s_lyTarget;
static double s_lzTarget;
static double s_lrxTarget;
static double s_lryTarget;
static double s_lrzTarget;
static double s_lelbowTarget;
static double s_lkx;
static double s_lky;
static double s_lkz;
static double s_lkrx;
static double s_lkry;
static double s_lkrz;
static double s_lcx;
static double s_lcy;
static double s_lcz;
static double s_lcrx;
static double s_lcry;
static double s_lcrz;
static bool s_bMsgFlag2 = false;
static double s_rq1;
static double s_rq2;
static double s_rq3;
static double s_rq4;
static double s_rq5;
static double s_rq6;
static double s_rq7;
static double s_rx;
static double s_ry;
static double s_rz;
static double s_rrx;
static double s_rry;
static double s_rrz;
static double s_relbow;
static double s_rFT1;
static double s_rFT2;
static double s_rFT3;
static double s_rFT4;
static double s_rFT5;
static double s_rFT6;
static quint16 s_rjState1;
static quint16 s_rjState2;
static quint16 s_rjState3;
static quint16 s_rjState4;
static quint16 s_rjState5;
static quint16 s_rjState6;
static quint16 s_rjState7;
static quint16 s_rEnable;
static quint16 s_rerrorClean;
static quint16 s_rcontrolMode;
static quint16 s_rftMode;
static quint16 s_rtriggerSignal;
static quint16 s_rperiodMode;
static double s_rperiodValue;
static double s_rtoolmass;
static double s_rtoolComX;
static double s_rtoolComY;
static double s_rtoolComZ;
static double s_rqTarget1;
static double s_rqTarget2;
static double s_rqTarget3;
static double s_rqTarget4;
static double s_rqTarget5;
static double s_rqTarget6;
static double s_rqTarget7;
static double s_rxTarget;
static double s_ryTarget;
static double s_rzTarget;
static double s_rrxTarget;
static double s_rryTarget;
static double s_rrzTarget;
static double s_relbowTarget;
static double s_rkx;
static double s_rky;
static double s_rkz;
static double s_rkrx;
static double s_rkry;
static double s_rkrz;
static double s_rcx;
static double s_rcy;
static double s_rcz;
static double s_rcrx;
static double s_rcry;
static double s_rcrz;

static double armParam[2][7] = {{0}};

RobotServerCmd::RobotServerCmd(quint16 usPort,
                               int argc,
                               char** argv,
                               QObject* parent)
    : argc_(argc), argv_(argv), RobotServer(usPort, parent)
{
    m_ros_spin_thread =
        std::thread{std::bind(&RobotServerCmd::rosSpinThread, this)};

    m_smTcp = new QSharedMemory("sm_tcp", this);
    m_smTcp->create(1024 * 1024);  // rgb*2

    QFile f("armParam.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF) {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss) {
            ++col;
            armParam[row][col] = x.toFloat();
        }
    }
}

RobotServerCmd::~RobotServerCmd()
{
    qDebug() << __FUNCTION__ << " called";
    rclcpp::shutdown();
}

void RobotServerCmd::call_processTcp(unsigned int dwSize)
{
    QMetaObject::invokeMethod(this, "processTcp", Qt::QueuedConnection,
                              Q_ARG(unsigned int, dwSize));
}

void RobotServerCmd::processTcp(unsigned int dwSize)
{
    m_smTcp->lock();

    MsgRecv* left = (MsgRecv*)(m_smTcp->data());
    MsgRecv1* left1 = (MsgRecv1*)((char*)m_smTcp->data() + sizeof(MsgRecv));
    MsgRecv2* left2 = (MsgRecv2*)((char*)m_smTcp->data() + sizeof(MsgRecv) +
                                  sizeof(MsgRecv1));

    // qDebug() << " RobotServerCmd::processTcp  left: " << left->FT1 << " "
    //          << left->q2;

    MsgRecv* right = (MsgRecv*)((char*)m_smTcp->data() + sizeof(MsgRecv) +
                                sizeof(MsgRecv1) + sizeof(MsgRecv2));
    MsgRecv1* right1 =
        (MsgRecv1*)((char*)m_smTcp->data() + sizeof(MsgRecv) +
                    sizeof(MsgRecv1) + sizeof(MsgRecv2) + sizeof(MsgRecv));
    MsgRecv2* right2 = (MsgRecv2*)((char*)m_smTcp->data() + sizeof(MsgRecv) +
                                   sizeof(MsgRecv1) + sizeof(MsgRecv2) +
                                   sizeof(MsgRecv) + sizeof(MsgRecv1));

    // qDebug() << " RobotServerCmd::processTcp right: " << right->FT1 << " "
    //          << right->q2;

    m_smTcp->unlock();

    m_mutexVar.lock();

    s_lq1 = left->q1;
    s_lq2 = left->q2;
    s_lq3 = left->q3;
    s_lq4 = left->q4;
    s_lq5 = left->q5;
    s_lq6 = left->q6;
    s_lq7 = left->q7;
    s_lx = left->x;
    s_ly = left->y;
    s_lz = left->z;
    s_lrx = left->rx;
    s_lry = left->ry;
    s_lrz = left->rz;
    s_lelbow = left->elbow;
    s_lFT1 = left->FT1;
    s_lFT2 = left->FT2;
    s_lFT3 = left->FT3;
    s_lFT4 = left->FT4;
    s_lFT5 = left->FT5;
    s_lFT6 = left->FT6;

    s_ljState1 = left1->jState1;
    s_ljState2 = left1->jState2;
    s_ljState3 = left1->jState3;
    s_ljState4 = left1->jState4;
    s_ljState5 = left1->jState5;
    s_ljState6 = left1->jState6;
    s_ljState7 = left1->jState7;

    s_lEnable = left1->Enable;
    s_lerrorClean = left1->errorClean;
    s_lcontrolMode = left1->controlMode;
    s_lftMode = left1->ftMode;
    s_ltriggerSignal = left1->triggerSignal;
    s_lperiodMode = left1->periodMode;

    s_lperiodValue = left2->periodValue;
    s_ltoolmass = left2->toolmass;
    s_ltoolComX = left2->toolComX;
    s_ltoolComY = left2->toolComY;
    s_ltoolComZ = left2->toolComZ;
    s_lqTarget1 = left2->qTarget1;
    s_lqTarget2 = left2->qTarget2;
    s_lqTarget3 = left2->qTarget3;
    s_lqTarget4 = left2->qTarget4;
    s_lqTarget5 = left2->qTarget5;
    s_lqTarget6 = left2->qTarget6;
    s_lqTarget7 = left2->qTarget7;
    s_lxTarget = left2->xTarget;
    s_lyTarget = left2->yTarget;
    s_lzTarget = left2->zTarget;
    s_lrxTarget = left2->rxTarget;
    s_lryTarget = left2->ryTarget;
    s_lrzTarget = left2->rzTarget;
    s_lelbowTarget = left2->elbowTarget;
    s_lkx = left2->kx;
    s_lky = left2->ky;
    s_lkz = left2->kz;
    s_lkrx = left2->krx;
    s_lkry = left2->kry;
    s_lkrz = left2->krz;
    s_lcx = left2->cx;
    s_lcy = left2->cy;
    s_lcz = left2->cz;
    s_lcrx = left2->crx;
    s_lcry = left2->cry;
    s_lcrz = left2->crz;

    s_rq1 = right->q1;
    s_rq2 = right->q2;
    s_rq3 = right->q3;
    s_rq4 = right->q4;
    s_rq5 = right->q5;
    s_rq6 = right->q6;
    s_rq7 = right->q7;
    s_rx = right->x;
    s_ry = right->y;
    s_rz = right->z;
    s_rrx = right->rx;
    s_rry = right->ry;
    s_rrz = right->rz;
    s_relbow = right->elbow;
    s_rFT1 = right->FT1;
    s_rFT2 = right->FT2;
    s_rFT3 = right->FT3;
    s_rFT4 = right->FT4;
    s_rFT5 = right->FT5;
    s_rFT6 = right->FT6;

    s_rjState1 = right1->jState1;
    s_rjState2 = right1->jState2;
    s_rjState3 = right1->jState3;
    s_rjState4 = right1->jState4;
    s_rjState5 = right1->jState5;
    s_rjState6 = right1->jState6;
    s_rjState7 = right1->jState7;

    s_rEnable = right1->Enable;
    s_rerrorClean = right1->errorClean;
    s_rcontrolMode = right1->controlMode;
    s_rftMode = right1->ftMode;
    s_rtriggerSignal = right1->triggerSignal;
    s_rperiodMode = right1->periodMode;

    s_rperiodValue = right2->periodValue;
    s_rtoolmass = right2->toolmass;
    s_rtoolComX = right2->toolComX;
    s_rtoolComY = right2->toolComY;
    s_rtoolComZ = right2->toolComZ;
    s_rqTarget1 = right2->qTarget1;
    s_rqTarget2 = right2->qTarget2;
    s_rqTarget3 = right2->qTarget3;
    s_rqTarget4 = right2->qTarget4;
    s_rqTarget5 = right2->qTarget5;
    s_rqTarget6 = right2->qTarget6;
    s_rqTarget7 = right2->qTarget7;
    s_rxTarget = right2->xTarget;
    s_ryTarget = right2->yTarget;
    s_rzTarget = right2->zTarget;
    s_rrxTarget = right2->rxTarget;
    s_rryTarget = right2->ryTarget;
    s_rrzTarget = right2->rzTarget;
    s_relbowTarget = right2->elbowTarget;
    s_rkx = right2->kx;
    s_rky = right2->ky;
    s_rkz = right2->kz;
    s_rkrx = right2->krx;
    s_rkry = right2->kry;
    s_rkrz = right2->krz;
    s_rcx = right2->cx;
    s_rcy = right2->cy;
    s_rcz = right2->cz;
    s_rcrx = right2->crx;
    s_rcry = right2->cry;
    s_rcrz = right2->crz;

    m_mutexVar.unlock();

    if (nullptr != m_socketCur && m_socketCur->isValid()) {
        QVariantMap mapJson, mapLeft, mapRight;
        QVariantList listLeftJointAngles, listRightJointAngle;
        QVariantList listLeftJointAnglesTarget, listRightJointAnglesTarget;
        QVariantList listLeftEndPosTarget, listRightEndPosTarget;
        QVariantList listLeftJointState, listRightJointState;
        QVariantList listLeftCC, listRightCC;
        QVariantList listLefttoolCom, listRighttoolCom;

        mapJson.insert("action", "robotState");
        mapJson.insert("isConnected", 1);
        mapLeft.insert("x", left->x);
        mapLeft.insert("y", left->y);
        mapLeft.insert("z", left->z);
        mapLeft.insert("rx", left->rx);
        mapLeft.insert("ry", left->ry);
        mapLeft.insert("rz", left->rz);
        mapLeft.insert("elbow", left->elbow);
        mapLeft.insert("touch", 0);
        mapLeft.insert("tool", 0);
        listLeftJointAngles << left->q1 << left->q2 << left->q3 << left->q4
                            << left->q5 << left->q6 << left->q7;
        listLeftJointState << left1->jState1 << left1->jState2 << left1->jState3
                           << left1->jState4 << left1->jState5 << left1->jState6
                           << left1->jState7;
        mapLeft.insert("jointAngles", listLeftJointAngles);
        mapLeft.insert("jointState", listLeftJointState);

        listLeftJointAnglesTarget << left2->qTarget1 << left2->qTarget2
                                  << left2->qTarget3 << left2->qTarget4
                                  << left2->qTarget5 << left2->qTarget6
                                  << left2->qTarget7;
        listLeftEndPosTarget << left2->xTarget << left2->yTarget
                             << left2->zTarget << left2->rxTarget
                             << left2->ryTarget << left2->rzTarget
                             << left2->elbowTarget;
        listLeftCC << s_lkx << s_lky << s_lkz << s_lkrx << s_lkry << s_lkrz
                   << s_lcx << s_lcy << s_lcz << s_lcrx << s_lcry << s_lcrz;
        mapLeft.insert("jointAnglesTarget", listLeftJointAnglesTarget);  // new
        mapLeft.insert("endPosTarget", listLeftEndPosTarget);            // new
        mapLeft.insert("CC", listLeftCC);                                // new

        mapLeft.insert("lockState", s_lEnable);
        mapLeft.insert("errorState", s_lerrorClean);
        mapLeft.insert("moveMode", s_lperiodMode);
        mapLeft.insert("controlMode", s_lcontrolMode);      // new
        mapLeft.insert("ftMode", s_lftMode);                // new
        mapLeft.insert("triggerSignal", s_ltriggerSignal);  // new

        mapLeft.insert("moveSec", s_lperiodValue);
        mapLeft.insert("toolmass", s_ltoolmass);  // new
        listLefttoolCom << s_ltoolComX << s_ltoolComY << s_ltoolComZ;
        mapLeft.insert("toolSize", listLefttoolCom);  // new

        mapJson.insert("left", mapLeft);

        mapRight.insert("x", s_rx);
        mapRight.insert("y", s_ry);
        mapRight.insert("z", s_rz);
        mapRight.insert("rx", s_rrx);
        mapRight.insert("ry", s_rry);
        mapRight.insert("rz", s_rrz);
        mapRight.insert("elbow", s_relbow);
        mapRight.insert("touch", 0);
        mapRight.insert("tool", 0);
        listRightJointAngle << s_rq1 << s_rq2 << s_rq3 << s_rq4 << s_rq5
                            << s_rq6 << s_rq7;
        listRightJointState << s_rjState1 << s_rjState2 << s_rjState3
                            << s_rjState4 << s_rjState5 << s_rjState6
                            << s_rjState7;
        mapRight.insert("jointAngles", listRightJointAngle);
        mapRight.insert("jointState", listRightJointState);

        listRightJointAnglesTarget << s_rqTarget1 << s_rqTarget2 << s_rqTarget3
                                   << s_rqTarget4 << s_rqTarget5 << s_rqTarget6
                                   << s_rqTarget7;
        listRightEndPosTarget << s_rxTarget << s_ryTarget << s_rzTarget
                              << s_rrxTarget << s_rryTarget << s_rrzTarget
                              << s_relbowTarget;
        listRightCC << s_rkx << s_rky << s_rkz << s_rkrx << s_rkry << s_rkrz
                    << s_rcx << s_rcy << s_rcz << s_rcrx << s_rcry << s_rcrz;
        mapRight.insert("jointAnglesTarget",
                        listRightJointAnglesTarget);             // new
        mapRight.insert("endPosTarget", listRightEndPosTarget);  // new
        mapRight.insert("CC", listRightCC);                      // new

        mapRight.insert("lockState", s_rEnable);
        mapRight.insert("errorState", s_rerrorClean);
        mapRight.insert("moveMode", s_rperiodMode);
        mapRight.insert("controlMode", s_rcontrolMode);      // new
        mapRight.insert("ftMode", s_rftMode);                // new
        mapRight.insert("triggerSignal", s_rtriggerSignal);  // new

        mapRight.insert("moveSec", s_rperiodValue);
        mapRight.insert("toolmass", s_rtoolmass);  // new
        listRighttoolCom << s_rtoolComX << s_rtoolComY << s_rtoolComZ;
        mapRight.insert("toolSize", listRighttoolCom);  // new

        mapJson.insert("right", mapRight);

        QByteArray baRobotState =
            JsonUtil::packDataFE(mapJson, QByteArray());  // ef->fe

        //             if (nullptr != m_socketCur)
        //             {
        // //                qDebug() << "RobotServerCmd::onRecvRobotMsg2
        // m_socket is null";
        //                 m_socketCur->write(baRobotState);
        //                 m_socketCur->flush();
        //             }

        qint64 nWrite = 0;
        try {
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                // qDebug() << __FUNCTION__
                //          << "write robot_arm total: " << baRobotState.size()
                //          << "  ,write: " << nWrite;
            }
        } catch (...) {
            qDebug() << "write cloud exception";
        }
    }
}

void RobotServerCmd::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

int RobotServerCmd::parse(unsigned char* buf, qint64 len)
{
    int nStartIndex = 0;
    while (nStartIndex + 8 < len) {
        if (buf[nStartIndex] == 0xfe && buf[nStartIndex + 1] == 0xfe &&
            buf[nStartIndex + 2] == 0xfe && buf[nStartIndex + 3] == 0xfe) {

            qDebug() << "buf[nStartIndex+7]: " << buf[nStartIndex + 7];
            qDebug() << "buf[nStartIndex+6]: " << buf[nStartIndex + 6];
            qDebug() << "buf[nStartIndex+5]: " << buf[nStartIndex + 5];
            qDebug() << "buf[nStartIndex+4]: " << buf[nStartIndex + 4];

            int nJsonLen = int(buf[nStartIndex + 4]) << 24;
            nJsonLen += int(buf[nStartIndex + 5]) << 16;
            nJsonLen += int(buf[nStartIndex + 6]) << 8;
            nJsonLen += int(buf[nStartIndex + 7]);

            // qDebug() << "parse jsonLen: " << nJsonLen;

            if (nStartIndex + 8 + nJsonLen > len) {
                return 0;
            }

            QByteArray ba;
            for (int i = 0; i < nJsonLen; ++i) {
                ba.append(buf[nStartIndex + 8 + i]);
            }
            // qDebug() << "parse ba: " << ba;
            QVariantMap mapJson;
            if (!JsonUtil::byteArray2VarMap(ba, mapJson)) {
                return nStartIndex + 4;
            } else {
                processJson(mapJson);
                return nStartIndex + 8 + nJsonLen;
            }
        } else {
            nStartIndex++;
        }
    }
    return nStartIndex;
}

void RobotServerCmd::processJson(const QVariantMap& json)
{
    QString strAction = json.value("action").toString();
    QString strSerial = json.value("serial", "").toString();

    qDebug() << "RobotServerCmd cmd strSerial: " << strSerial;

    QVariantMap mapLeftCmd;

    {
        unique_lock<mutex> lk(m_mutexVar);
        mapLeftCmd.insert("q1", s_lq1);
        mapLeftCmd.insert("q2", s_lq2);
        mapLeftCmd.insert("q3", s_lq3);
        mapLeftCmd.insert("q4", s_lq4);
        mapLeftCmd.insert("q5", s_lq5);
        mapLeftCmd.insert("q6", s_lq6);
        mapLeftCmd.insert("q7", s_lq7);
        mapLeftCmd.insert("x2", s_lx);
        mapLeftCmd.insert("y2", s_ly);
        mapLeftCmd.insert("z2", s_lz);
        mapLeftCmd.insert("rx2", s_lrx);
        mapLeftCmd.insert("ry2", s_lry);
        mapLeftCmd.insert("rz2", s_lrz);
        mapLeftCmd.insert("elbow2", s_lelbow);
    }

    QVariantMap mapRightCmd;
    {
        unique_lock<mutex> lk(m_mutexVar);
        mapRightCmd.insert("q1", s_rq1);
        mapRightCmd.insert("q2", s_rq2);
        mapRightCmd.insert("q3", s_rq3);
        mapRightCmd.insert("q4", s_rq4);
        mapRightCmd.insert("q5", s_rq5);
        mapRightCmd.insert("q6", s_rq6);
        mapRightCmd.insert("q7", s_rq7);
        mapRightCmd.insert("x2", s_rx);
        mapRightCmd.insert("y2", s_ry);
        mapRightCmd.insert("z2", s_rz);
        mapRightCmd.insert("rx2", s_rrx);
        mapRightCmd.insert("ry2", s_rry);
        mapRightCmd.insert("rz2", s_rrz);
        mapRightCmd.insert("elbow2", s_relbow);
    }

    if ("armZero" == strAction) {
        qDebug() << "action: " << strAction;
    } else if ("holdArm" == strAction) {
        qDebug() << __FUNCTION__ << "action: " << strAction;
        qDebug() << __FUNCTION__ << "json: " << json;
        int nUnlock = json.value("unlock").toInt();
        QString strArm = json.value("arm").toString();
        qDebug() << "unlock = " << nUnlock;

        unique_lock<mutex> lk(m_mutexVar);
        mapLeftCmd.insert("x2", s_lxTarget);
        mapLeftCmd.insert("y2", s_lyTarget);
        mapLeftCmd.insert("z2", s_lzTarget);
        mapLeftCmd.insert("rx2", s_lrxTarget);
        mapLeftCmd.insert("ry2", s_lryTarget);
        mapLeftCmd.insert("rz2", s_lrzTarget);
        mapLeftCmd.insert("elbow2", s_lelbowTarget);
        mapLeftCmd.insert("toolWeight", s_ltoolmass);
        mapLeftCmd.insert("toolComX", s_ltoolComX);
        mapLeftCmd.insert("toolComY", s_ltoolComY);
        mapLeftCmd.insert("toolComZ", s_ltoolComZ);

        mapLeftCmd.insert("periodValue", s_lperiodValue);

        mapLeftCmd.insert("kx", s_lkx);
        mapLeftCmd.insert("ky", s_lky);
        mapLeftCmd.insert("kz", s_lkz);
        mapLeftCmd.insert("krx", s_lkrx);
        mapLeftCmd.insert("kry", s_lkry);
        mapLeftCmd.insert("krz", s_lkrz);
        mapLeftCmd.insert("cx", s_lcx);
        mapLeftCmd.insert("cy", s_lcy);
        mapLeftCmd.insert("cz", s_lcz);
        mapLeftCmd.insert("crx", s_lcrx);
        mapLeftCmd.insert("cry", s_lcry);
        mapLeftCmd.insert("crz", s_lcrz);

        mapRightCmd.insert("x2", s_rxTarget);
        mapRightCmd.insert("y2", s_ryTarget);
        mapRightCmd.insert("z2", s_rzTarget);
        mapRightCmd.insert("rx2", s_rrxTarget);
        mapRightCmd.insert("ry2", s_rryTarget);
        mapRightCmd.insert("rz2", s_rrzTarget);
        mapRightCmd.insert("elbow2", s_relbowTarget);
        mapRightCmd.insert("toolWeight", s_rtoolmass);
        mapRightCmd.insert("toolComX", s_rtoolComX);
        mapRightCmd.insert("toolComY", s_rtoolComY);
        mapRightCmd.insert("toolComZ", s_rtoolComZ);

        mapRightCmd.insert("periodValue", s_rperiodValue);

        mapRightCmd.insert("kx", s_rkx);
        mapRightCmd.insert("ky", s_rky);
        mapRightCmd.insert("kz", s_rkz);
        mapRightCmd.insert("krx", s_rkrx);
        mapRightCmd.insert("kry", s_rkry);
        mapRightCmd.insert("krz", s_rkrz);
        mapRightCmd.insert("cx", s_rcx);
        mapRightCmd.insert("cy", s_rcy);
        mapRightCmd.insert("cz", s_rcz);
        mapRightCmd.insert("crx", s_rcrx);
        mapRightCmd.insert("cry", s_rcry);
        mapRightCmd.insert("crz", s_rcrz);

        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);

        mapArmCmd.insert("unlock", nUnlock);
        mapArmCmd.insert("arm", strArm);

        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "holdArm", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        // Tcp::instance()->writeDuoArmDirect(mapLeftCmd, mapRightCmd);
        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                // qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                //          << "  ,write: " << nWrite;
            }
        }
    } else if ("armControl" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        qDebug() << "\n";

        QVariantMap mapLeft = json.value("left", QVariantMap()).toMap();
        QVariantMap mapRight = json.value("right", QVariantMap()).toMap();
        if (!mapLeft.isEmpty()) {
            unique_lock<mutex> lk(m_mutexVar);

            float x = mapLeft.value("x", 0).toFloat() + armParam[0][0];
            float y = mapLeft.value("y", 0).toFloat() + armParam[0][1];
            float z = mapLeft.value("z", 0).toFloat() + armParam[0][2];
            float rx = mapLeft.value("rx", 0).toFloat() + armParam[0][3];
            float ry = mapLeft.value("ry", 0).toFloat() + armParam[0][4];
            float rz = mapLeft.value("rz", 0).toFloat() + armParam[0][5];
            float elbow = mapLeft.value("elbow", 0).toFloat() + armParam[0][6];

            mapLeftCmd.insert("x2", x);
            mapLeftCmd.insert("y2", y);
            mapLeftCmd.insert("z2", z);
            mapLeftCmd.insert("rx2", rx);
            mapLeftCmd.insert("ry2", ry);
            mapLeftCmd.insert("rz2", rz);
            mapLeftCmd.insert("elbow2", elbow);

            mapLeftCmd.insert("toolWeight", s_ltoolmass);
            mapLeftCmd.insert("toolComX", s_ltoolComX);
            mapLeftCmd.insert("toolComY", s_ltoolComY);
            mapLeftCmd.insert("toolComZ", s_ltoolComZ);

            mapLeftCmd.insert("periodValue", s_lperiodValue);

            mapLeftCmd.insert("kx", s_lkx);
            mapLeftCmd.insert("ky", s_lky);
            mapLeftCmd.insert("kz", s_lkz);
            mapLeftCmd.insert("krx", s_lkrx);
            mapLeftCmd.insert("kry", s_lkry);
            mapLeftCmd.insert("krz", s_lkrz);
            mapLeftCmd.insert("cx", s_lcx);
            mapLeftCmd.insert("cy", s_lcy);
            mapLeftCmd.insert("cz", s_lcz);
            mapLeftCmd.insert("crx", s_lcrx);
            mapLeftCmd.insert("cry", s_lcry);
            mapLeftCmd.insert("crz", s_lcrz);
        }
        if (!mapRight.isEmpty()) {
            unique_lock<mutex> lk(m_mutexVar);

            float x = mapRight.value("x", 0).toFloat() + armParam[1][0];
            float y = mapRight.value("y", 0).toFloat() + armParam[1][1];
            float z = mapRight.value("z", 0).toFloat() + armParam[1][2];
            float rx = mapRight.value("rx", 0).toFloat() + armParam[1][3];
            float ry = mapRight.value("ry", 0).toFloat() + armParam[1][4];
            float rz = mapRight.value("rz", 0).toFloat() + armParam[1][5];
            float elbow = mapRight.value("elbow", 0).toFloat() + armParam[1][6];

            mapRightCmd.insert("x2", x);
            mapRightCmd.insert("y2", y);
            mapRightCmd.insert("z2", z);
            mapRightCmd.insert("rx2", rx);
            mapRightCmd.insert("ry2", ry);
            mapRightCmd.insert("rz2", rz);
            mapRightCmd.insert("elbow2", elbow);

            mapRightCmd.insert("toolWeight", s_rtoolmass);
            mapRightCmd.insert("toolComX", s_rtoolComX);
            mapRightCmd.insert("toolComY", s_rtoolComY);
            mapRightCmd.insert("toolComZ", s_rtoolComZ);

            mapRightCmd.insert("periodValue", s_rperiodValue);

            mapRightCmd.insert("kx", s_rkx);
            mapRightCmd.insert("ky", s_rky);
            mapRightCmd.insert("kz", s_rkz);
            mapRightCmd.insert("krx", s_rkrx);
            mapRightCmd.insert("kry", s_rkry);
            mapRightCmd.insert("krz", s_rkrz);
            mapRightCmd.insert("cx", s_rcx);
            mapRightCmd.insert("cy", s_rcy);
            mapRightCmd.insert("cz", s_rcz);
            mapRightCmd.insert("crx", s_rcrx);
            mapRightCmd.insert("cry", s_rcry);
            mapRightCmd.insert("crz", s_rcrz);
        }

        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);
        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "armControl", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                // qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                //          << "  ,write: " << nWrite;
            }
        }
    } else if ("armJointControl" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        qDebug() << "\n";
        QString strArm = json.value("arm").toString();
        int nJoint = json.value("joint").toInt();
        double r = json.value("r").toDouble();

        if ("left" == strArm) {
            mapLeftCmd.insert(QString("q%1").arg(nJoint + 1), r);
        } else if ("right" == strArm) {
            mapRightCmd.insert(QString("q%1").arg(nJoint + 1), r);
        }

        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);
        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "armJointControl", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());

            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                // qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                //          << "  ,write: " << nWrite;
            }
        }
    } else if ("toolControl" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
    } else if ("stop" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;

        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);
        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "stop", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("platformControl" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
    } else if ("setTrigger" == strAction) {
        int nLeftTrigger = json.value("left").toMap().value("trigger").toInt();
        int nRightTrigger = json.value("right").toMap().value("trigger").toInt();

        // if (nLeftTrigger == 1 && nRightTrigger == 1) {
        //     // RsWorker::instance()->trigger(nLeftTrigger, nRightTrigger);
        //     std::vector<std::string> strVec;
        //     strVec.push_back(QString("%1").arg(nLeftTrigger).toStdString());
        //     strVec.push_back(QString("%1").arg(nRightTrigger).toStdString());
        //     m_robotCmdNode->sendParamMsg("setGenRParam", strVec);
        // }

        if (nLeftTrigger == 1 && nRightTrigger == 1) {
            QString strLeftTrigger = QString("%1").arg(nLeftTrigger);
            QString strRightTrigger = QString("%2").arg(nRightTrigger);
            std::vector<std::string> strVec;
            strVec.push_back(strLeftTrigger.toStdString());
            strVec.push_back(strRightTrigger.toStdString());
            m_robotCmdNode->sendParamMsg("trigger_servercmd", strVec);
        }

        unique_lock<mutex> lk(m_mutexVar);

        mapLeftCmd.insert("x2", s_lxTarget);
        mapLeftCmd.insert("y2", s_lyTarget);
        mapLeftCmd.insert("z2", s_lzTarget);
        mapLeftCmd.insert("rx2", s_lrxTarget);
        mapLeftCmd.insert("ry2", s_lryTarget);
        mapLeftCmd.insert("rz2", s_lrzTarget);
        mapLeftCmd.insert("elbow2", s_lelbowTarget);
        mapLeftCmd.insert("toolWeight", s_ltoolmass);
        mapLeftCmd.insert("toolComX", s_ltoolComX);
        mapLeftCmd.insert("toolComY", s_ltoolComY);
        mapLeftCmd.insert("toolComZ", s_ltoolComZ);

        mapLeftCmd.insert("periodValue", s_lperiodValue);

        mapLeftCmd.insert("kx", s_lkx);
        mapLeftCmd.insert("ky", s_lky);
        mapLeftCmd.insert("kz", s_lkz);
        mapLeftCmd.insert("krx", s_lkrx);
        mapLeftCmd.insert("kry", s_lkry);
        mapLeftCmd.insert("krz", s_lkrz);
        mapLeftCmd.insert("cx", s_lcx);
        mapLeftCmd.insert("cy", s_lcy);
        mapLeftCmd.insert("cz", s_lcz);
        mapLeftCmd.insert("crx", s_lcrx);
        mapLeftCmd.insert("cry", s_lcry);
        mapLeftCmd.insert("crz", s_lcrz);
        mapLeftCmd.insert("trigger", nLeftTrigger);

        mapRightCmd.insert("x2", s_rxTarget);
        mapRightCmd.insert("y2", s_ryTarget);
        mapRightCmd.insert("z2", s_rzTarget);
        mapRightCmd.insert("rx2", s_rrxTarget);
        mapRightCmd.insert("ry2", s_rryTarget);
        mapRightCmd.insert("rz2", s_rrzTarget);
        mapRightCmd.insert("elbow2", s_relbowTarget);
        mapRightCmd.insert("toolWeight", s_rtoolmass);
        mapRightCmd.insert("toolComX", s_rtoolComX);
        mapRightCmd.insert("toolComY", s_rtoolComY);
        mapRightCmd.insert("toolComZ", s_rtoolComZ);

        mapRightCmd.insert("periodValue", s_rperiodValue);

        mapRightCmd.insert("kx", s_rkx);
        mapRightCmd.insert("ky", s_rky);
        mapRightCmd.insert("kz", s_rkz);
        mapRightCmd.insert("krx", s_rkrx);
        mapRightCmd.insert("kry", s_rkry);
        mapRightCmd.insert("krz", s_rkrz);
        mapRightCmd.insert("cx", s_rcx);
        mapRightCmd.insert("cy", s_rcy);
        mapRightCmd.insert("cz", s_rcz);
        mapRightCmd.insert("crx", s_rcrx);
        mapRightCmd.insert("cry", s_rcry);
        mapRightCmd.insert("crz", s_rcrz);
        mapRightCmd.insert("trigger", nRightTrigger);

        // Tcp::instance()->writeDuoArmDirect(mapLeftCmd, mapRightCmd);
        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);
        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "setTrigger", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("setArmEnd" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        int handstate = json.value("state").toInt();
        qDebug() << "handstate = " << handstate;


        // only for test
        // if (handstate == 0) {
        //     m_robotCmdNode->sendPcCmd("pinch", "spin45");
        // } else if (handstate == 1) {
        //     m_robotCmdNode->sendPcCmd("pinch", "songkai");
        // } else if (handstate == 2) {
        //     m_robotCmdNode->sendPcCmd("pinch", "180x9");
        // } else if (handstate == 3) {
        //     m_robotCmdNode->sendPcCmd("pinch", "spinBack45");
        // } else if (handstate == 4) {
        //     m_robotCmdNode->sendPcCmd("pinch", "stop");
        // } else if (handstate == 5) {
        //     m_robotCmdNode->sendPcCmd("pinch", "resetPos");
        // }

        if (handstate >= 1 && handstate <= 3)
        {
            m_robotCmdNode->clawControl(0, handstate);
        }

        if (handstate == 7) {
            m_robotCmdNode->sendPcCmd("pinch", "spin45");
        } else if (handstate == 8) {
            m_robotCmdNode->sendPcCmd("pinch", "songkai");
        } else if (handstate == 9) {
            m_robotCmdNode->sendPcCmd("pinch", "180x9");
        } else if (handstate == 10) {
            m_robotCmdNode->sendPcCmd("pinch", "spinBack45");
        }

        if (handstate == 17) {
            m_robotCmdNode->sendPcCmd("pinch", "spin45");
        } else if (handstate == 18) {
            m_robotCmdNode->sendPcCmd("pinch", "songkai");
        } else if (handstate == 19) {
            m_robotCmdNode->sendPcCmd("pinch", "180x9");
        } else if (handstate == 20) {
            m_robotCmdNode->sendPcCmd("pinch", "spinBack45");
        }

        if (handstate == 30) {
            m_robotCmdNode->sendPcCmd("pinch", "spin45");
        } else if (handstate == 31) {
            m_robotCmdNode->sendPcCmd("pinch", "songkai");
        } else if (handstate == 32) {
            m_robotCmdNode->sendPcCmd("pinch", "180x9");
        } else if (handstate == 33) {
            m_robotCmdNode->sendPcCmd("pinch", "spinBack45");
        } else if (handstate == 34) {
            m_robotCmdNode->sendPcCmd("pinch", "stop");
        } else if (handstate == 35) {
            m_robotCmdNode->sendPcCmd("pinch", "resetPos");
        }

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("controlMode" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        int contolModeValue = json.value("state").toInt();
        QString strArm = json.value("arm").toString();
        qDebug() << strArm << ": contolModeValue = " << contolModeValue;

        ////change0405 mode =0 1 endmode mode == 0 jointmode
        // if (1 == contolModeValue)
        //{
        //	if ("left" == strArm)
        //	{
        //		Tcp::instance()->setLeftControlMode(1);
        //		//                Tcp::instance()->writeLeftDirect(cmd);
        //	}
        //	else if ("right" == strArm)
        //	{
        //		Tcp::instance()->setRightControlMode(1);
        //		//                Tcp::instance()->writeRightDirect(cmd);
        //	}
        // }
        // else if (0 == contolModeValue)
        //{
        //	if ("left" == strArm)
        //	{
        //		Tcp::instance()->setLeftControlMode(0);
        //	}
        //	else if ("right" == strArm)
        //	{
        //		Tcp::instance()->setRightControlMode(0);
        //	}
        // }

        // Tcp::instance()->writeDuoArmDirect(mapLeftCmd, mapRightCmd);
        //  writeDuoArmDirect
        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);

        mapArmCmd.insert("contolModeValue", contolModeValue);
        mapArmCmd.insert("arm", strArm);

        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "controlMode", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("resetArm" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        int errorCleanValue = json.value("state").toInt();
        QString strArm = json.value("arm").toString();
        qDebug() << strArm << ": errorCleanValue = " << errorCleanValue;

        // change0405 mode == 1 ErrClean mode == 0 no ErrClean
        // if (1 == errorCleanValue)
        //{
        //	if ("left" == strArm)
        //	{
        //		Tcp::instance()->setLeftErrClean(1);
        //	}
        //	else if ("right" == strArm)
        //	{
        //		Tcp::instance()->setRightErrClean(1);
        //	}
        // }
        // else if (0 == errorCleanValue)
        //{
        //	if ("left" == strArm)
        //	{
        //		Tcp::instance()->setLeftErrClean(0);
        //	}
        //	else if ("right" == strArm)
        //	{
        //		Tcp::instance()->setRightErrClean(0);
        //	}
        // }

        // Tcp::instance()->writeDuoArmDirect(mapLeftCmd, mapRightCmd);
        //  writeDuoArmDirect
        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);

        mapArmCmd.insert("errorCleanValue", errorCleanValue);
        mapArmCmd.insert("arm", strArm);

        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "resetArm", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("resetArmEnd" == strAction) {
        unique_lock<mutex> lk(m_mutexVar);

        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        int ftModeValue = json.value("state").toInt();
        QString strArm = json.value("arm").toString();
        qDebug() << strArm << ": ftModeValue = " << ftModeValue;

        mapLeftCmd.insert("x2", s_lxTarget);
        mapLeftCmd.insert("y2", s_lyTarget);
        mapLeftCmd.insert("z2", s_lzTarget);
        mapLeftCmd.insert("rx2", s_lrxTarget);
        mapLeftCmd.insert("ry2", s_lryTarget);
        mapLeftCmd.insert("rz2", s_lrzTarget);
        mapLeftCmd.insert("elbow2", s_lelbowTarget);

        mapRightCmd.insert("x2", s_rxTarget);
        mapRightCmd.insert("y2", s_ryTarget);
        mapRightCmd.insert("z2", s_rzTarget);
        mapRightCmd.insert("rx2", s_rrxTarget);
        mapRightCmd.insert("ry2", s_rryTarget);
        mapRightCmd.insert("rz2", s_rrzTarget);
        mapRightCmd.insert("elbow2", s_relbowTarget);

        ////change0405 ftModeValue == 1 autoset ftzero ftModeValue == 0 noneset
        /// ftzero
        // if (1 == ftModeValue)
        //{
        //	if ("left" == strArm)
        //	{
        //		Tcp::instance()->setLeftFTMode(1);
        //	}
        //	else if ("right" == strArm)
        //	{
        //		Tcp::instance()->setRightFTMode(1);
        //	}
        // }
        // else if (0 == ftModeValue)
        //{
        //	if ("left" == strArm)
        //	{
        //		Tcp::instance()->setLeftFTMode(0);
        //	}
        //	else if ("right" == strArm)
        //	{
        //		Tcp::instance()->setRightFTMode(0);
        //	}
        // }

        // Tcp::instance()->writeDuoArmDirect(mapLeftCmd, mapRightCmd);

        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);

        mapArmCmd.insert("ftModeValue", ftModeValue);
        mapArmCmd.insert("arm", strArm);

        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "resetArmEnd", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("setMoveSec" == strAction) {
        unique_lock<mutex> lk(m_mutexVar);

        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;
        int moveSecMode = json.value("mode").toInt();
        QString strArm = json.value("arm").toString();
        double periodValue = json.value("seconds").toDouble();
        qDebug() << strArm << ": moveSecMode = " << moveSecMode;
        qDebug() << strArm << ": periodValue = " << periodValue;

        mapLeftCmd.insert("x2", s_lxTarget);
        mapLeftCmd.insert("y2", s_lyTarget);
        mapLeftCmd.insert("z2", s_lzTarget);
        mapLeftCmd.insert("rx2", s_lrxTarget);
        mapLeftCmd.insert("ry2", s_lryTarget);
        mapLeftCmd.insert("rz2", s_lrzTarget);
        mapLeftCmd.insert("elbow2", s_lelbowTarget);
        mapLeftCmd.insert("toolWeight", s_ltoolmass);
        mapLeftCmd.insert("toolComX", s_ltoolComX);
        mapLeftCmd.insert("toolComY", s_ltoolComY);
        mapLeftCmd.insert("toolComZ", s_ltoolComZ);

        mapLeftCmd.insert("kx", s_lkx);
        mapLeftCmd.insert("ky", s_lky);
        mapLeftCmd.insert("kz", s_lkz);
        mapLeftCmd.insert("krx", s_lkrx);
        mapLeftCmd.insert("kry", s_lkry);
        mapLeftCmd.insert("krz", s_lkrz);
        mapLeftCmd.insert("cx", s_lcx);
        mapLeftCmd.insert("cy", s_lcy);
        mapLeftCmd.insert("cz", s_lcz);
        mapLeftCmd.insert("crx", s_lcrx);
        mapLeftCmd.insert("cry", s_lcry);
        mapLeftCmd.insert("crz", s_lcrz);

        mapLeftCmd.insert("periodValue", s_lperiodValue);

        mapRightCmd.insert("x2", s_rxTarget);
        mapRightCmd.insert("y2", s_ryTarget);
        mapRightCmd.insert("z2", s_rzTarget);
        mapRightCmd.insert("rx2", s_rrxTarget);
        mapRightCmd.insert("ry2", s_rryTarget);
        mapRightCmd.insert("rz2", s_rrzTarget);
        mapRightCmd.insert("elbow2", s_relbowTarget);
        mapRightCmd.insert("toolWeight", s_rtoolmass);
        mapRightCmd.insert("toolComX", s_rtoolComX);
        mapRightCmd.insert("toolComY", s_rtoolComY);
        mapRightCmd.insert("toolComZ", s_rtoolComZ);

        mapRightCmd.insert("kx", s_rkx);
        mapRightCmd.insert("ky", s_rky);
        mapRightCmd.insert("kz", s_rkz);
        mapRightCmd.insert("krx", s_rkrx);
        mapRightCmd.insert("kry", s_rkry);
        mapRightCmd.insert("krz", s_rkrz);
        mapRightCmd.insert("cx", s_rcx);
        mapRightCmd.insert("cy", s_rcy);
        mapRightCmd.insert("cz", s_rcz);
        mapRightCmd.insert("crx", s_rcrx);
        mapRightCmd.insert("cry", s_rcry);
        mapRightCmd.insert("crz", s_rcrz);

        mapRightCmd.insert("periodValue", s_rperiodValue);

        // ////change0405 SecMode == 1 maunal set ftzero SecMode == 0 auto set
        // /// ftzero
        // if (1 == moveSecMode)
        // {
        // 	if ("left" == strArm)
        // 	{
        // 		mapLeftCmd.insert("periodValue", periodValue);
        // 		Tcp::instance()->setLeftSecMode(1);
        // 	}
        // 	else if ("right" == strArm)
        // 	{
        // 		mapRightCmd.insert("periodValue", periodValue);
        // 		Tcp::instance()->setRightSecMode(1);
        // 	}
        // }
        // else if (0 == moveSecMode)
        // {
        // 	if ("left" == strArm)
        // 	{
        // 		Tcp::instance()->setLeftSecMode(0);
        // 	}
        // 	else if ("right" == strArm)
        // 	{
        // 		Tcp::instance()->setRightSecMode(0);
        // 	}
        // }

        strArm == "left" ? mapLeftCmd.insert("periodValue", periodValue) : mapRightCmd.insert("periodValue", periodValue);

        // Tcp::instance()->writeDuoArmDirect(mapLeftCmd, mapRightCmd);
        QVariantMap mapArmCmd;
        mapArmCmd.insert("left", mapLeftCmd);
        mapArmCmd.insert("right", mapRightCmd);

        mapArmCmd.insert("moveSecMode", moveSecMode);
        mapArmCmd.insert("arm", strArm);

        // writeDuoArmDirect
        m_robotCmdNode->sendPcCmd(
            "setMoveSec", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

        if (nullptr != m_socketCur && m_socketCur->isValid()) {
            QVariantMap mapJson;
            mapJson.insert("action", strAction);
            mapJson.insert("serial", strSerial);
            mapJson.insert("result", "0000");
            mapJson.insert("errmsg", "ok");

            QByteArray baRobotState = JsonUtil::packData(mapJson, QByteArray());
            //            m_curSock->write(baRobotState);
            // change0425
            qint64 nWrite = 0;
            while (nullptr != m_socketCur && m_socketCur->isValid() &&
                   nWrite < baRobotState.size()) {
                nWrite += m_socketCur->write(baRobotState.mid(nWrite));
                qDebug() << __FUNCTION__ << " total: " << baRobotState.size()
                         << "  ,write: " << nWrite;
            }
        }
    } else if ("operateControl" == strAction) {
        qDebug() << "recv action: " << strAction;
        qDebug() << "recv json: " << json;

        int nOperate = json.value("operate", -1).toInt();
        if (-1 != nOperate) {
            if (5 == nOperate)  // jiajin
            {
                // m_robotCmdNode->sendPcCmd("pinch", "180x9");
            } else if (6 == nOperate)  // songkai
            {
                // m_robotCmdNode->sendPcCmd("pinch", "songkai");
            }

            int nExecStep = json.value("execStep", -1).toInt();

            if (nExecStep == 0) {
                // RsWorker::instance()->setGenRParam(nOperate, strSerial);
                std::vector<std::string> strVec;
                strVec.push_back(QString("%1").arg(nOperate).toStdString());
                strVec.push_back(strSerial.toStdString());
                m_robotCmdNode->sendParamMsg("setGenRParam", strVec);
            } else if (nExecStep > 0) {
                // RsWorker::instance()->opreate(nExecStep, strSerial);
                std::vector<std::string> strVec;
                strVec.push_back(QString("%1").arg(nExecStep).toStdString());
                strVec.push_back(strSerial.toStdString());
                m_robotCmdNode->sendParamMsg("operate", strVec);
            }
        }
    }
}

void RobotServerCmd::call_onGenRReady(const QByteArray& baData)
{
    QMetaObject::invokeMethod(this, "onGenRReady", Qt::QueuedConnection,
                              Q_ARG(QByteArray, baData));
}

void RobotServerCmd::onGenRReady(const QByteArray& baData)
{
    qint64 nWrite = 0;
    while (nullptr != m_socketCur && m_socketCur->isValid() &&
           nWrite < baData.size()) {
        nWrite += m_socketCur->write(baData.mid(nWrite));
        qDebug() << __FUNCTION__ << " total: " << baData.size()
                 << "  ,write: " << nWrite;
    }
}
